#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include "Eigen/Dense"
#include <vector>
#include <fstream>
#include <cmath>
#include <algorithm>
#include <nlohmann/json.hpp>
#include "../include/client.h"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/bool.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>

// 使用 nlohmann::json
using json = nlohmann::json;
int DEBUG = false;
// 定义三维点结构
struct Point_3D {
    double x;
    double y;
    double z;
    Point_3D() : x(0), y(0), z(0) {}
    Point_3D(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
};

// 将二维点添加高度维度
std::vector<Point_3D> add_height_dimension(const std::vector<Point>& waypoints, double fixed_height) {
    std::vector<Point_3D> points;
    for (const auto& point : waypoints) {
        points.emplace_back(point.x, point.y, fixed_height);
    }
    return points;
}

// 获取路径点
std::vector<Point_3D> get_waypoints(const std::vector<Point_3D>& boardpoints, double height = -30, double spacing = 20) {
    if (height >= 0) {
        height = -height;
    }

    // 提取矩形边界点
    std::vector<double> x_coords, y_coords;
    for (const auto& point : boardpoints) {
        x_coords.push_back(point.x);
        y_coords.push_back(point.y);
    }

    // 找到矩形边界
    double min_x = *std::min_element(x_coords.begin(), x_coords.end());
    double max_x = *std::max_element(x_coords.begin(), x_coords.end());
    double min_y = *std::min_element(y_coords.begin(), y_coords.end());
    double max_y = *std::max_element(y_coords.begin(), y_coords.end());

    // 生成点阵
    std::vector<Point> grid_points;
    for (double x = min_x; x <= max_x; x += spacing) {
        for (double y = min_y; y <= max_y; y += spacing) {
            grid_points.emplace_back(x, y);
        }
    }

    // 计算中心点
    double center_x = (min_x + max_x) / 2.0;
    double center_y = (min_y + max_y) / 2.0;
    Point center_point(center_x, center_y);

    std::cout << "[DEBUG] Center Point: (" << center_x << ", " << center_y << ")" << std::endl;

    int url_index = 0;
    // 模拟路径排序（这里可以调用实际的排序函数）
    std::vector<Point> sort_points;
    sort_points = get_sorted_result(grid_points,center_point,url_index);
    // sort_points = grid_points;

    // 转换为三维路径点
    std::vector<Point_3D> waypoints = add_height_dimension(sort_points, height);
    return waypoints;
}

// B样条基函数，作为普通函数
double N(int i, int k, double t, const std::vector<double>& knots) {
    if (k == 0) {
        return (knots[i] <= t && t < knots[i + 1]) ? 1.0 : 0.0;
    } else {
        double coef1 = (t - knots[i]) / (knots[i + k] - knots[i]);
        double coef2 = (knots[i + k + 1] - t) / (knots[i + k + 1] - knots[i + 1]);

        if (knots[i + k] == knots[i]) coef1 = 0.0; // 避免除以0
        if (knots[i + k + 1] == knots[i + 1]) coef2 = 0.0; // 避免除以0

        return coef1 * N(i, k - 1, t, knots) + coef2 * N(i + 1, k - 1, t, knots);
    }
}

// B样条插值
std::vector<Point_3D> b_spline_interpolation(const std::vector<Point_3D>& waypoints, int num_interpolated_points = 100) {
    int n = waypoints.size() - 1; // 控制点数量
    int k = 3;                   // B样条的阶数（3表示三次样条）
    int m = n + k + 1;           // 节点向量的数量

    // 生成均匀的节点向量
    std::vector<double> knots(m + 1);
    for (int i = 0; i <= m; ++i) {
        if (i <= k)
            knots[i] = 0.0;
        else if (i > k && i < m - k)
            knots[i] = (i - k) / (double)(m - 2 * k);
        else
            knots[i] = 1.0;
    }

    // 存储插值点
    std::vector<Point_3D> interpolated_points;

    for (int j = 0; j < num_interpolated_points; ++j) {
        double t = (double)j / (num_interpolated_points - 1); // 参数范围 [0, 1]
        double x = 0.0, y = 0.0, z = 0.0;

        for (int i = 0; i <= n; ++i) {
            double basis = N(i, k, t, knots); // 调用普通函数 N
            x += basis * waypoints[i].x;
            y += basis * waypoints[i].y;
            z += basis * waypoints[i].z;
        }

    interpolated_points.emplace_back(x, y, z);
    }

    return interpolated_points;
}

// 从 JSON 文件中读取 boardpoints
std::vector<Point_3D> read_boardpoints_from_json(const std::string& filename,std::string key_name="boardpoints") {
    std::vector<Point_3D> boardpoints;

    // 打开 JSON 文件
    std::ifstream file(filename);
    if (!file.is_open()) {
        throw std::runtime_error("Failed to open JSON file: " + filename);
    }

    // 解析 JSON
    json j;
    file >> j;

    // 提取点数据
    for (const auto& point : j[key_name]) {
        double x = point["x"];
        double y = point["y"];
        double z = point["z"];
        boardpoints.emplace_back(x, y, z);
    }

    return boardpoints;
}


// 从 JSON 文件中读取普通params
double read_normal_params_from_json(const std::string& filename,std::string key_name="height") {
    double params;

    // 打开 JSON 文件
    std::ifstream file(filename);
    if (!file.is_open()) {
        throw std::runtime_error("Failed to open JSON file: " + filename);
    }

    // 解析 JSON
    json j;
    file >> j;

    params = j[key_name];

    return params;
}

class InterpolatedPointPublisher : public rclcpp::Node {
public:
    InterpolatedPointPublisher(const std::vector<Point_3D>& interpolated_points)
        : Node("interpolated_point_publisher"), interpolated_points_(interpolated_points), current_index_(0) {
        // 创建发布器
        publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/airsim/target_point", 10);

        // 创建订阅器，监听目标是否到达
        subscriber_ = this->create_subscription<std_msgs::msg::Bool>(
            "/airsim/goal_reached", 10,
            std::bind(&InterpolatedPointPublisher::on_goal_reached, this, std::placeholders::_1));
        publisher_answer = this->create_publisher<std_msgs::msg::Bool>("/airsim/goal_reached_received", 10);

        // 定时器，用于周期性地发布当前点
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(20),  // 每 20 毫秒发布一次当前点
            std::bind(&InterpolatedPointPublisher::publish_current_point, this));

        // 初始化时发布第一个点
        publish_current_point();
    }

private:
    // 收到目标到达消息的回调函数
    void on_goal_reached(const std_msgs::msg::Bool::SharedPtr msg) {
        if (msg->data) {  // 确认目标已到达
            RCLCPP_INFO(this->get_logger(), "Goal reached. Preparing to publish next point...");
            for (int i = 0; i < 10; ++i) // 10次冗余发送
                publish_answer_message(publisher_answer);
            ++current_index_;  // 切换到下一个点
            if (current_index_ >= interpolated_points_.size()) {
                RCLCPP_INFO(this->get_logger(), "All points have been published.");
                rclcpp::shutdown();  // 发布完所有点后关闭节点
            }
        }
    }

    void publish_answer_message(rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr publisher) {
        auto answer_msg = std_msgs::msg::Bool();  // 构造消息
        answer_msg.data = true;  // 设置数据
        publisher->publish(answer_msg);  // 发布消息
    }

    // 定时发布当前点
    void publish_current_point() {
        if (current_index_ >= interpolated_points_.size()) {
            return;  // 所有点已经发布完毕
        }

        const auto& point = interpolated_points_[current_index_];

        // 构造消息
        geometry_msgs::msg::PoseStamped pose_msg;
        pose_msg.header.stamp = this->get_clock()->now();
        pose_msg.header.frame_id = "map";

        pose_msg.pose.position.x = point.x;
        pose_msg.pose.position.y = point.y;
        pose_msg.pose.position.z = point.z;

        pose_msg.pose.orientation.x = 0.0;
        pose_msg.pose.orientation.y = 0.0;
        pose_msg.pose.orientation.z = 0.0;
        pose_msg.pose.orientation.w = 1.0;

        // 发布消息
        publisher_->publish(pose_msg);

        RCLCPP_INFO(this->get_logger(), "Published current point [%zu]: (%.2f, %.2f, %.2f)", 
                    current_index_, point.x, point.y, point.z);
    }

    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr publisher_answer;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr subscriber_;
    rclcpp::TimerBase::SharedPtr timer_;

    std::vector<Point_3D> interpolated_points_;  // 插值点
    size_t current_index_;  // 当前正在发布的点索引
};

int record_waypoints(std::string filename,std::vector<Point_3D> waypoints)
{

    // 打开文件以写入数据
    std::ofstream outFile(filename);

    // 检查文件是否成功打开
    if (!outFile) {
        std::cerr << "无法打开文件进行写入！" << std::endl;
        return 1;
    }

    // 将每个点的坐标写入文件
    for (const auto& point : waypoints) {
        outFile << point.x << " " << point.y << " " << point.z << std::endl;
    }

    // 关闭文件
    outFile.close();

    std::cout << "Waypoints已成功记录到文件中。" << std::endl;

    return 0;
}

// 读取文件中的waypoints
int read_waypoints_from_txt(std::string filename, std::vector<Point_3D>& waypoints)
{
    // 打开文件以读取数据
    std::ifstream inFile(filename);

    // 检查文件是否成功打开
    if (!inFile) {
        std::cerr << "无法打开文件进行读取！" << std::endl;
        return 1;
    }

    // 读取每一行，解析为Point_3D对象
    std::string line;
    while (std::getline(inFile, line)) {
        std::istringstream stream(line);  // 使用字符串流解析每一行
        Point_3D point;
        if (stream >> point.x >> point.y >> point.z) {
            // 如果成功读取三个坐标值，添加到waypoints中
            waypoints.push_back(point);
        } else {
            std::cerr << "读取数据时发生错误，忽略该行数据：" << line << std::endl;
        }
    }

    // 关闭文件
    inFile.close();

    std::cout << "Waypoints已成功从文件中读取。" << std::endl;

    return 0;
}

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    // 从 JSON 文件中读取 boardpoints

    // 获取包的共享目录路径
    // std::string package_share_directory = ament_index_cpp::get_package_share_directory("airsim_target_point");
    std::string package_share_directory = SRC_PATH;
    std::cout << "package_share_directory: " << package_share_directory << std::endl;
    std::string filename = package_share_directory + "/config/params.json";
    std::vector<Point_3D> boardpoints = read_boardpoints_from_json(filename,"boardpoints");

    // // 生成路径点
    double height = read_normal_params_from_json(filename,"height");
    double spacing = read_normal_params_from_json(filename,"spacing");

    std::vector<Point_3D> waypoints;
    std::string laod_path = package_share_directory + "/config/waypoints.txt";
    if (std::filesystem::exists(laod_path)) {
        std::cout << "路径文件存在，直接读取" << std::endl;
        read_waypoints_from_txt(laod_path,waypoints);
    }
    else {
        waypoints = get_waypoints(boardpoints,height,spacing);
        record_waypoints(laod_path,waypoints);
    }
    std::vector<Point_3D> interpolated_points = b_spline_interpolation(waypoints, 100);
    // 打印路径点
    std::cout << "[INFO] Generated Waypoints:" << std::endl;
    for (const auto& point : interpolated_points) {
        std::cout << "(" << point.x << ", " << point.y << ", " << point.z << ")" << std::endl;
    }
        // 启动 ROS 2 节点并发布插值点
    rclcpp::spin(std::make_shared<InterpolatedPointPublisher>(interpolated_points));
    rclcpp::shutdown();
    
    return 0;
}
